#include "LSM6DSLTR.h"

static stc_lsm6dsl_t lsm6dsl;
lsm6dsl_callback_t lsm6dsl_cbs;

static uint8_t whoamI, rst;
//static int16_t acceleration_temp[3][ACC_COLL_CNT_MAX];
//static int16_t angular_rate_temp[3][ACC_COLL_CNT_MAX];
stmdev_ctx_t dev_ctx;

void lsm_sda_pin_set(uint8_t sta)
{
	if(sta == IIC_HIGH)
		ACC_SDA_HIGH();
	else
		ACC_SDA_LOW();
}

void lsm_scl_pin_set(uint8_t sta)
{
	if(sta == IIC_HIGH)
		ACC_SCL_HIGH();
	else
		ACC_SCL_LOW();
}

uint8_t lsm_sda_pin_get(void)
{
	uint8_t r_data = 0;
	
	r_data = ACC_SDA_GET();
	
	return r_data;
}

void lsm_sda_pin_dir(uint8_t dir)
{
	if(dir == SDA_OUT)
		ACC_SDA_OUT();
	else
		ACC_SDA_IN();
}

stc_soft_iic_t lsm_iic_t = 
{
	.sda_pin_set = lsm_sda_pin_set,
	.scl_pin_set = lsm_scl_pin_set,
	.sda_pin_get = lsm_sda_pin_get,
	.sda_pin_dir = lsm_sda_pin_dir,
	.delay_us    = hdl_delay_us,
};

static void platform_delay(uint32_t millisec) 
{
    rt_thread_mdelay(millisec);
}


static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len) {
    uint16_t Selve_Add = 0xD4;
    hdl_iic_master_write(&lsm_iic_t, Selve_Add, reg, (uint8_t *) bufp, len);
    return 0;
}

static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len) {
    uint16_t Selve_Add = 0xD4;
    hdl_iic_master_read(&lsm_iic_t, Selve_Add, reg, bufp, len);
    return 0;
}

void LSM6DSLTR_Init(void) {
    //初始化驱动配置
    dev_ctx.write_reg = platform_write;
    dev_ctx.read_reg = platform_read;
    dev_ctx.mdelay = platform_delay;

    platform_delay(BOOT_TIME);

    whoamI = 0;
    lsm6dsl_device_id_get(&dev_ctx, &whoamI);
	
	lsm6dsl_cbs = 0;

    if (whoamI != LSM6DSL_ID) {
        rt_kprintf("LSM6DS INIT Err...\r\n"); /*manage here device not found */
//        MyLpUart_SendString("LSM6DS INIT Err...\r\n");
        return;
    }
    //开启编程模式 恢复默认设置
    lsm6dsl_reset_set(&dev_ctx, PROPERTY_ENABLE);
    do {
        lsm6dsl_reset_get(&dev_ctx, &rst);
    } while (rst);

    //高性能模式设置
    lsm6dsl_xl_power_mode_set(&dev_ctx, LSM6DSL_XL_HIGH_PERFORMANCE);
    lsm6dsl_gy_power_mode_set(&dev_ctx, LSM6DSL_GY_HIGH_PERFORMANCE);
    /* 启用块数据更新 读取完成后才更新数据 */
    lsm6dsl_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);

    /* 设置加速度 输出功率 12.5hZ */
    lsm6dsl_xl_data_rate_set(&dev_ctx, LSM6DSL_XL_ODR_6k66Hz);
    //陀螺仪设置 输出功率 12.5hZ
    lsm6dsl_gy_data_rate_set(&dev_ctx, LSM6DSL_GY_ODR_6k66Hz);
    /*设置满量程 2g*/
    lsm6dsl_xl_full_scale_set(&dev_ctx, LSM6DSL_2g);
    //陀螺仪满量程设置 2000dps
    lsm6dsl_gy_full_scale_set(&dev_ctx, LSM6DSL_2000dps);
    /* Configure filtering chain(No aux interface) */
    /* 加速度计 模拟滤波器设置 */
    lsm6dsl_xl_filter_analog_set(&dev_ctx, LSM6DSL_XL_ANA_BW_400Hz);
    /* Accelerometer - LPF1 path ( LPF2 not used )*/
//    lsm6dsl_xl_lp1_bandwidth_set(&dev_ctx, LSM6DSL_XL_LP1_ODR_DIV_4);
    /* Accelerometer - LPF1 + LPF2 path */
    lsm6dsl_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSL_XL_LOW_NOISE_LP_ODR_DIV_100);
    /* Accelerometer - High Pass / Slope path */
//    lsm6dsl_xl_reference_mode_set(&dev_ctx, PROPERTY_DISABLE);
//    lsm6dsl_xl_hp_bandwidth_set(&dev_ctx, LSM6DSL_XL_HP_ODR_DIV_100);
    /* 陀螺仪-滤波配置 */
    lsm6dsl_gy_band_pass_set(&dev_ctx, LSM6DSL_HP_260mHz_LP1_STRONG);


    //fifo模式设置
    lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_BYPASS_MODE);
    //fifo模式设置
    lsm6dsl_fifo_xl_batch_set(&dev_ctx, LSM6DSL_FIFO_XL_NO_DEC);//fifo加速度写入频率设置
//    lsm6dsl_fifo_gy_batch_set(&dev_ctx, LSM6DSL_FIFO_GY_NO_DEC);//fifo陀螺仪写入频率设置
    lsm6dsl_fifo_data_rate_set(&dev_ctx, LSM6DSL_FIFO_12Hz5);//FIFO ODR选择

//    //中断1设置，watermark full中断
//    lsm6dsl_int1_route_t int1val = {0, 0, 0};
//    int1val.int1_full_flag = 1;        //开阀值中断，fifo存储到缓存深度时触发
//    lsm6dsl_pin_int1_route_set(&dev_ctx, int1val);
//    lsm6dsl_fifo_watermark_set(&dev_ctx, ACC_FIFO_WATERMASK);//FIFO水印电平设置，阈值设置，缓存深度
//    lsm6dsl_fifo_stop_on_wtm_set(&dev_ctx, 1);//fifo缓存深度存满时，不再存储
//    //lsm6dsl_fifo_xl_gy_8bit_format_set(&dev_ctx, 1);//在FIFO中为XL和陀螺启用MSByte记忆，缓存宽度
//    lsm6dsl_block_data_update_set(&dev_ctx, 1);//fifo模式ctrl3_c bdu必须为1,直到读取MSB和LSB后才更新输出寄存器
//    lsm6dsl_auto_increment_set(&dev_ctx, 1);//fifo模式ctrl3_c if_inc必须为1,通过串行接口(I2C或SPI)进行多字节访问时自动增加寄存器地址。
//    lsm6dsl_ctrl3_c_t ctrl3_c;
//    platform_read(NULL, LSM6DSL_CTRL3_C, (uint8_t *) &ctrl3_c, 1);
//    //lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_FIFO_MODE);
//
//    //中断2设置，唤醒中断
////	uint8_t temp = 0x90;		//唤醒中断
////	platform_write(LSM6DSL_TAP_CFG, &temp, 1);
////	temp = 0;
////	platform_write(LSM6DSL_WAKE_UP_DUR, &temp, 1);
////	lsm6dsl_wkup_threshold_set(&dev_ctx, 0x20);
////	lsm6dsl_int2_route_t int2val ={0, 0, 0};
////	int2val.int2_wu = 1;
////	lsm6dsl_pin_int2_route_set(&dev_ctx, int2val);
//
////    rt_kprintf("LEFT LSM6DS INIT OK...\r\n");
    rt_kprintf("LSM6DS INIT OK...\r\n");
}

void lsm6dsl_callback_cbs_register(lsm6dsl_callback_t pCBS)
{
	if(lsm6dsl_cbs == 0)
	{
		lsm6dsl_cbs = pCBS;
	}
}

void lsm6dsl_proc(void)
{
	lsm6dsl_reg_t reg;
	int16_t acc_buf[3] = {0};
//	int16_t gyro_buf[3] = {0};
	
	switch(lsm6dsl.step)
	{
		case LSM6DSL_IDLE:
			break;
		case LSM6DSL_INIT:
			break;
		case LSM6DSL_START:
			lsm6dsl_status_reg_get(&dev_ctx, &reg.status_reg);
			if((reg.status_reg.xlda) && (reg.status_reg.gda))
				lsm6dsl.step = LSM6DSL_READ;
			break;
		case LSM6DSL_READ:
			// 获取加速度数据
			rt_memset(acc_buf, 0x00, 3 * sizeof(int16_t));
			lsm6dsl_acceleration_raw_get(&dev_ctx, acc_buf);
			lsm6dsl.acc_x = lsm6dsl_from_fs2g_to_mg(acc_buf[0]) / 1000.0f * 9.8f;
			lsm6dsl.acc_y = lsm6dsl_from_fs2g_to_mg(acc_buf[1]) / 1000.0f * 9.8f;
			lsm6dsl.acc_z = lsm6dsl_from_fs2g_to_mg(acc_buf[2]) / 1000.0f * 9.8f;
		
//			// 获取陀螺仪数据
//			lsm6dsl_angular_rate_raw_get(&dev_ctx, gyro_buf);
//			lsm6dsl.gyro_x = lsm6dsl_from_fs2000dps_to_mdps(gyro_buf[0]) / 1000.0f;
//			lsm6dsl.gyro_y = lsm6dsl_from_fs2000dps_to_mdps(gyro_buf[1]) / 1000.0f;
//			lsm6dsl.gyro_z = lsm6dsl_from_fs2000dps_to_mdps(gyro_buf[2]) / 1000.0f;
		
			if(lsm6dsl_cbs)
			{
				lsm6dsl_cbs(lsm6dsl);
			}
		
			lsm6dsl.step = LSM6DSL_START;
		break;
		default:
			break;
	}
}

void lsm6dsl_start(void)
{
	lsm6dsl.step = LSM6DSL_START;
}

////static int16_t acceleration_mg[3][ACC_COLL_CNT_MAX];
////static int16_t angular_rate_mdps[3][ACC_COLL_CNT_MAX];
//static int16_t data_raw_acceleration[3];
//static int16_t data_raw_angular_rate[3];
//static float acceleration_mg[3];
//static float acceleration_velocity[3];
//static float angular_rate_mdps[3];
//static float angular_velocity[3];
//static int16_t data_raw_temperature;
//static float temperature_degC;



//void LSM6DSLTRTask(void) {
//    /* 仅当新值可用时才读取输出 */
//    lsm6dsl_reg_t reg;
//    lsm6dsl_status_reg_get(&dev_ctx, &reg.status_reg);

//    //读取加速度数据
//    if (reg.status_reg.xlda) {
////        for (int i = 0; i < ACC_COLL_CNT_MAX; ++i) {
////            /* 读取加速度数据 */
////            memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
////            lsm6dsl_acceleration_raw_get(&dev_ctx, data_raw_acceleration);
////            acceleration_temp[X][i] = (int16_t) lsm6dsl_from_fs2g_to_mg(
////                    data_raw_acceleration[0]);
////            acceleration_temp[Y][i] = (int16_t) lsm6dsl_from_fs2g_to_mg(
////                    data_raw_acceleration[1]);
////            acceleration_temp[Z][i] = (int16_t) lsm6dsl_from_fs2g_to_mg(
////                    data_raw_acceleration[2]);
////        }
////        acceleration_mg[0] = (int16_t) IntFilter(acceleration_temp[0], ACC_COLL_CNT_MAX, 2);
////        acceleration_mg[1] = (int16_t) IntFilter(acceleration_temp[1], ACC_COLL_CNT_MAX, 2);
////        acceleration_mg[2] = (int16_t) IntFilter(acceleration_temp[2], ACC_COLL_CNT_MAX, 2);
////        acceleration_velocity[0] = acceleration_mg[0] / 1000.0f * 9.8f;
////        acceleration_velocity[1] = acceleration_mg[1] / 1000.0f * 9.8f;
////        acceleration_velocity[2] = acceleration_mg[2] / 1000.0f * 9.8f;
//        memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
//        lsm6dsl_acceleration_raw_get(&dev_ctx, data_raw_acceleration);
//        acceleration_mg[0] = lsm6dsl_from_fs2g_to_mg(data_raw_acceleration[0]);
//        acceleration_mg[1] = lsm6dsl_from_fs2g_to_mg(data_raw_acceleration[1]);
//        acceleration_mg[2] = lsm6dsl_from_fs2g_to_mg(data_raw_acceleration[2]);
//        acceleration_velocity[0] = acceleration_mg[0] / 1000.0f * 9.8f;
//        acceleration_velocity[1] = acceleration_mg[1] / 1000.0f * 9.8f;
//        acceleration_velocity[2] = acceleration_mg[2] / 1000.0f * 9.8f;
//    }

//    if (reg.status_reg.gda) {
//        for (int i = 0; i < ACC_COLL_CNT_MAX; ++i) {
//            /* 读取陀螺仪数据 */
//            memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));
//            lsm6dsl_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate);
//            angular_rate_temp[X][i] = (int16_t) lsm6dsl_from_fs2000dps_to_mdps(
//                    data_raw_angular_rate[0]);
//            angular_rate_temp[Y][i] = (int16_t) lsm6dsl_from_fs2000dps_to_mdps(
//                    data_raw_angular_rate[1]);
//            angular_rate_temp[Z][i] = (int16_t) lsm6dsl_from_fs2000dps_to_mdps(
//                    data_raw_angular_rate[2]);
//        }
//        angular_rate_mdps[0] = (int16_t) IntFilter(angular_rate_temp[0], ACC_COLL_CNT_MAX, 2);
//        angular_rate_mdps[1] = (int16_t) IntFilter(angular_rate_temp[1], ACC_COLL_CNT_MAX, 2);
//        angular_rate_mdps[2] = (int16_t) IntFilter(angular_rate_temp[2], ACC_COLL_CNT_MAX, 2);
//        angular_velocity[0] = angular_rate_mdps[0] / 1000.0f;
//        angular_velocity[1] = angular_rate_mdps[1] / 1000.0f;
//        angular_velocity[2] = angular_rate_mdps[2] / 1000.0f;
//        rt_kprintf(
//                "Angular rate [mdps]:%4.2f,%4.2f,%4.2f\r\n",
//                angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);
//        rt_kprintf("Angular velocity [d/s]:%4.2f,%4.2f,%4.2f\r\n",
//                   angular_velocity[0], angular_velocity[1], angular_velocity[2]);
////        tx_com( tx_buffer, strlen( (char const *)tx_buffer ) );
//    }

//    if (reg.status_reg.tda) {
//        /* 读取温度数据 */
//        memset(&data_raw_temperature, 0x00, sizeof(int16_t));
//        lsm6dsl_temperature_raw_get(&dev_ctx, &data_raw_temperature);
//        temperature_degC = lsm6dsl_from_lsb_to_celsius(
//                data_raw_temperature);
////        dbg_printf( "Temperature [degC]:%6.2f\r\n",
////                temperature_degC );
////        tx_com( tx_buffer, strlen( (char const *)tx_buffer ) );
//    }
//    rt_kprintf("\r\n");
//    stress_detection.acc_x = acceleration_mg[0];
//    stress_detection.acc_y = acceleration_mg[1];
//    stress_detection.acc_z = acceleration_mg[2];
//    stress_detection.gyr_x = angular_velocity[0];
//    stress_detection.gyr_y = angular_velocity[1];
//    stress_detection.gyr_z = angular_velocity[2];
//    stress_detection.acc_temperature = temperature_degC;
//    //更新姿态数据
//    IMU_Update(&stress_detection.Roll, &stress_detection.Pitch, &stress_detection.Yaw);
//    //计算校准后的姿态数据
//    stress_detection.Roll_output = stress_detection.Roll - stress_detection.Roll_offset;
//    stress_detection.Pitch_output = stress_detection.Pitch - stress_detection.Pitch_offset;
//    stress_detection.Yaw_output = stress_detection.Yaw - stress_detection.Yaw_offset;
//    //临界处理
//    if (stress_detection.Yaw_output > 180) {
//        stress_detection.Yaw_output = stress_detection.Yaw_output - 360;
//    }
//    if (stress_detection.Yaw_output < -180) {
//        stress_detection.Yaw_output = stress_detection.Yaw_output + 360;
//    }
//    if (stress_detection.Pitch_output > 90) {
//        stress_detection.Pitch_output = 180 - stress_detection.Pitch_output;
//    }
//    if (stress_detection.Pitch_output < -90) {
//        stress_detection.Pitch_output = -180 - stress_detection.Pitch_output;
//    }
//    if (stress_detection.Roll_output > 180) {
//        stress_detection.Roll_output = 360 - stress_detection.Roll_output;
//    }
//    if (stress_detection.Roll_output < -180) {
//        stress_detection.Roll_output = -360 - stress_detection.Roll_output;
//    }

//    rt_event_send(&event, (1 << 4));

//}

///**
// * 姿态角校准
// */
//void attitudeSetZero(void) {
//    stress_detection.Roll_offset = stress_detection.Roll;
//    stress_detection.Pitch_offset = stress_detection.Pitch;
//    stress_detection.Yaw_offset = stress_detection.Yaw;
//}


//void LSM6DSLTR_thread_entry(void *parameter) {
//    //建立傅里叶变换信号表，使用查表法计算，提高计算速度
//    create_sin_tab(SIN_TAB);
////    rt_thread_delay(50);
//    LSM6DSLTR_Init();
//    stress_detection.Roll_offset = 0;
//    stress_detection.Pitch_offset = 0;
//    stress_detection.Yaw_offset = 0;

//    for (;;) {
//        LSM6DSLTRTask();
//        //延时10ms
//        rt_thread_mdelay(2);
//    }
//}
